﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using UtilityLibrary;
using Microsoft.Xna.Framework;
using Animation;
using System.Collections;
using FarseerPhysics;
using FarseerPhysics.Common;

namespace MapLibrary.AILibrary.Pathfinding
{
    public class NodeComparer : IComparer<Node>
    {
        public int Compare(Node n1, Node n2)
        {
            return n1.Compare(n2);
        }
    }

    public class Node
    {
        private List<Node> neighbors;
        private Vector2 position;

        public Vector2 Position { get { return position; } }
        public bool Blocked { get; set; }

        public double G { get; set; }
        public double H { get; set; }
        public double F { get; set; }
        public Node Parent { get; set; }
        public bool InOpen { get; set; }
        public bool InClosed { get; set; }

        public List<Node> Neighbors { get { return neighbors; } }

        public Node(float x, float y, bool blocked = false)
        {
            position = new Vector2(x, y);

            Blocked = blocked;

            neighbors = new List<Node>();
        }

        public void AddNeighbor(Node n)
        {
            neighbors.Add(n);
        }

        public void RemoveNeighbor(Node n)
        {
            neighbors.Remove(n);
        }

        public double Distance(Node n)
        {
            float dx = position.X - n.position.X;
            float dy = position.Y - n.position.Y;
            return (float)Math.Sqrt(dx * dx + dy * dy);
        }

        public double DistanceSquared(Node n)
        {
            float dx = position.X - n.position.X;
            float dy = position.Y - n.position.Y;
            return dx * dx + dy * dy;
        }

        public void Reset()
        {
            Parent = null;
            F = float.MaxValue;
            G = float.MaxValue;
            H = float.MaxValue;
            InOpen = false;
            InClosed = false;
        }

        public int Compare(Node other)
        {
            if (F > other.F) return 1;
            else if (F < other.F) return -1;
            return 0;
        }
    }

    public class Graph
    {
        private int width;
        private int height;
        private float x;
        private float y;
        private float step;

        private PriorityQueueB<Node> PQ;
        private List<Node> closed;

        private List<Node> nodes;

        public List<Node> Nodes { get { return nodes; } }

        public Graph(int width, int height, float step, float x, float y)
        {
            this.width = width;
            this.height = height;
            this.x = x;
            this.y = y;
            this.step = step;

            PQ = new PriorityQueueB<Node>(new NodeComparer());
            nodes = new List<Node>();
            closed = new List<Node>();

            for (int i = 0; i < height; i++)
            {
                float yPos = y + (i * step);
                for (int j = 0; j < width; j++)
                {
                    float xPos = x + (j * step);
                    Node n = new Node(xPos, yPos);
                    nodes.Add(n);
                }
            }

            for (int i = 0; i < height; i++)
            {
                for (int j = 0; j < width; j++)
                {
                    Node n = nodes[(i * width) + j];
                    List<Node> neighbors = n.Neighbors;
                    if (j > 0)
                    {
                        neighbors.Add(nodes[(i * width) + (j - 1)]);
                        if (i > 0)
                        {
                            neighbors.Add(nodes[((i - 1) * width) + (j - 1)]);
                            neighbors.Add(nodes[((i - 1) * width) + j]);
                        }
                        if (i < height - 1)
                        {
                            neighbors.Add(nodes[((i + 1) * width) + (j - 1)]);
                        }
                    }
                    if (j < width - 1)
                    {
                        neighbors.Add(nodes[(i * width) + (j + 1)]);
                        if (i > 0)
                        {
                            neighbors.Add(nodes[((i-1) * width) + (j+1)]);
                        }
                        if (i < height - 1)
                        {
                            neighbors.Add(nodes[((i + 1) * width) + (j + 1)]);
                        }
                    }
                    if (i < height - 1)
                    {
                        neighbors.Add(nodes[((i + 1) * width) + j]);
                    }
                }
            }
        }

        public void BlockNodesInGround(LevelCollisionRegion ground)
        {
            for (int k = 0; k < nodes.Count; k++)
            {
                Node n = nodes[k];
 
                Vertices v = ground.GetCollisionData().Hull;
                Vector2 pos = n.Position;
                if (v.PointInPolygon(ref pos) >= 0)
                {
                    n.Blocked = true;
                }
                
            }
        }

        public Node GetClosestNode(float x, float y)
        {
            x -= this.x;
            y -= this.y;

            x /= step;
            y /= step;

            x = (float)Math.Floor(x);
            y = (float)Math.Floor(y);

            if (x < 0 || x > width - 1 || y < 0 || y > height - 1)
            {
                return null;
            }

            return nodes[(int)((y * width) + x)];
        }

        public Node GetClosestNode(Vector2 pos)
        {
            return GetClosestNode(pos.X, pos.Y);
        }

        public bool GetPath(Sprite s1, Sprite s2, ref List<Node> path)
        {
            Node n1 = GetClosestNode(s1.Position);
            Node n2 = GetClosestNode(s2.Position);

            return GetPath(n1, n2, ref path);
        }

        public bool GetPath(Node n1, Node n2, ref List<Node> path, double maxDist = 1000000.0)
        {
            if (n1 == null || n2 == null)
            {
                return false;
            }
            else if (n1.Equals(n2))
            {
                path.Add(n1);
                return true;
            }

            double f,g,h;

            Reset();

            n1.G = 0.0f;
            n1.H = n1.DistanceSquared(n2);
            n1.F = n1.G + n1.H;

            PQ.Push(n1);

            Node current;

            while (true)
            {
                if (PQ.Count < 1)
                {
                    return false;
                }

                current = PQ.Pop();
                current.InClosed = true;
                closed.Add(current);

                if (current.Equals(n2))
                {
                    BuildPath(n2, ref path);
                    return true;
                }

                List<Node> neighbors = current.Neighbors;
                foreach (Node n in neighbors)
                {
                    if (n.Blocked)
                    {
                        continue;
                    }
                    g = current.DistanceSquared(n) + current.G;
                    h = n.DistanceSquared(n2);
                    f = g + h;
                    if (f > maxDist)
                    {
                        return false;
                    }

                    if (n.InClosed)
                    {
                        if (f < n.F)
                        {
                            n.Parent = current;
                            n.G = g;
                            n.H = h;
                            n.F = f;

                            closed.Remove(n);
                            n.InClosed = false;
                            n.InOpen = true;
                            PQ.Push(n);
                        }
                    }
                    else if (n.InOpen)
                    {
                        if (f < n.F)
                        {
                            n.Parent = current;
                            n.G = g;
                            n.H = h;
                            n.F = f;

                            PQ.RemoveLocation(n);
                            PQ.Push(n);
                        }
                    }
                    else
                    {
                        n.G = g;
                        n.H = h;
                        n.F = f;
                        n.Parent = current;
                        n.InOpen = true;
                        PQ.Push(n);
                    }
                }
            }
        }

        private void BuildPath(Node n, ref List<Node> path)
        {
            path.Clear();

            Node current = n;
            path.Add(current);
         
            while (current.Parent != null)
            {
                current = current.Parent;
                path.Add(current);
            }

            path.Reverse();
        }

        public void Reset()
        {
            closed.Clear();
            PQ.Clear();

            foreach (Node n in nodes)
            {
                n.Reset();
            }
        }

        public void draw(SpriteBatch batch, WorldViewport viewport)
        {
            for (int i = 0; i < height; i++)
            {
                for (int j = 0; j < width; j++)
                {
                    Node node = nodes[i * width + j];
                    PrimitiveBrush.RenderPoint(batch, viewport.WorldToViewportCoords(node.Position));
                }
            }
        }
    }
}
